#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/LinearMath/Quaternion.h"

#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
using namespace std::chrono_literals;
class TFListener:public rclcpp::Node
{
public:
    TFListener():Node("tf_broadcaster_node")
    {
        RCLCPP_INFO(this->get_logger(),"坐标监听节点已创建！");
        buffer_=std::make_shared<tf2_ros::Buffer>(this->get_clock());
        tflistener_=std::make_shared<tf2_ros::TransformListener>(*buffer_,this);
        timer_=this->create_wall_timer(1s,std::bind(&TFListener::get_transform,this));
    }
private:
    void get_transform()
        {
            try
            {
                //geometry_msgs::msg::TransformStamped 
                //(const std::string &target_frame, 
                //const std::string &source_frame, 
                //const tf2::TimePoint &time) const
                const auto &ts=buffer_->lookupTransform("camera","laser",
                tf2::TimePointZero);
                const auto &tf=ts.transform.translation;    
                const auto &tr=ts.transform.rotation;    
                double yaw ,pitch,roll;
                tf2::getEulerYPR(tr,yaw,pitch,roll);
                RCLCPP_INFO(this->get_logger(),"父坐标系:%s",ts.header.frame_id.c_str());
                RCLCPP_INFO(this->get_logger(),"子坐标系:%s",ts.child_frame_id.c_str());
                RCLCPP_INFO(this->get_logger(),"坐标转换:(%f,%f,%f)",tf.x,tf.y,tf.z);
                RCLCPP_INFO(this->get_logger(),"角坐标变换:(%f,%f,%f)",yaw,pitch,roll);
            }
                //Possible exceptions tf2::LookupException,
            catch(const tf2::LookupException&e) //(conts std::exception &e)
            {
                RCLCPP_INFO(this->get_logger(),"异常提示:%s",e.what());
            }
        };
    

    std::shared_ptr<tf2_ros::Buffer>buffer_;
    std::shared_ptr<tf2_ros::TransformListener>tflistener_;   
    std::shared_ptr<rclcpp::TimerBase>timer_;   
};
int main(int argc,char*argv[])
{
    rclcpp::init(argc,argv);
    auto node=std::make_shared<TFListener>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
